#include "vtor_step_motor.h"

#ifdef __VTOR_STEP_MOTOR__

void VtorStepMotor_RotatePulse(VtorStepMotor* motor, int32_t pulse)
{
	motor->motorExpPulse += pulse;
}

void VtorStepMotor_SetPulse(VtorStepMotor* motor, int32_t pulse)
{
	motor->motorExpPulse = pulse;
}

void VtorStepMotor_Stop(VtorStepMotor* motor)
{
	motor->motorExpPulse = motor->motorCurPulse;
}

int32_t VtorStepMotor_GetPulse(VtorStepMotor* motor)
{
	return motor->motorCurPulse;
}

uint8_t VtorStepMotor_CheckReady(VtorStepMotor* motor)
{
	return motor->motorState == VtorStepMotorState_Stop;
}

void VtorStepMotor_Run(VtorStepMotor* motor)
{
	if(motor->motorCurPulse != motor->motorExpPulse)
	{
		const int16_t pinPhaseArray[]
			 = {0x1, 0x5, 0x4, 0x6, 0x2, 0xa, 0x8, 0x9};
		if(motor->motorCurPulse < motor->motorExpPulse)
		{
			motor->motorState = VtorStepMotorState_Forward;
			motor->motorCurPulse++;
		}
		else if(motor->motorCurPulse > motor->motorExpPulse)
		{
			motor->motorState = VtorStepMotorState_Reverse;
			motor->motorCurPulse--;
		}
		int8_t phaseIndex = ((motor->motorCurPulse % 8) + 8) % 8;
		for(int pinIdx = 0; pinIdx < 4; pinIdx++)
		{
			int8_t pinLevel = (pinPhaseArray[phaseIndex] >> pinIdx) & 0x01;
			VtorStepMotor_WritePinLevel(motor, pinIdx, pinLevel);
			//VtorPrintf("%d%d   ", pinIdx, pinLevel);
		}
	}
	else
	{
		if(motor->motorState != VtorStepMotorState_Stop)
		{
			motor->motorState = VtorStepMotorState_Stop;
			for(int pinIdx = 0; pinIdx < 4; pinIdx++)
			{
				int8_t pinLevel = 1;
				VtorStepMotor_WritePinLevel(motor, pinIdx, pinLevel);
				//VtorPrintf("%d%d   ", pinIdx, pinLevel);
			}
		}
	}
}

void VtorPulseMotor_Run(VtorStepMotor* motor)
{
	if(motor->motorCurPulse != motor->motorExpPulse)
	{
		motor->motorPulseLevel++;
		if(motor->motorCurPulse < motor->motorExpPulse)
		{
			if(motor->motorState != VtorStepMotorState_Forward)
			{
				// 控制方向
				VtorPulseMotor_WritePinLevel(
					motor, VtorPulseMotor_DirPin, 1);
				motor->motorState = VtorStepMotorState_Forward;
			}
			if(motor->motorPulseLevel % 2 == 0)
			{
				motor->motorCurPulse++;
			}
		}
		else if(motor->motorCurPulse > motor->motorExpPulse)
		{
			if(motor->motorState != VtorStepMotorState_Reverse)
			{
				VtorPulseMotor_WritePinLevel(
					motor, VtorPulseMotor_DirPin, 0);
				motor->motorState = VtorStepMotorState_Reverse;
			}
			if(motor->motorPulseLevel % 2 == 0)
			{
				motor->motorCurPulse--;
			}
		}
		// 控制脉冲
		VtorPulseMotor_WritePinLevel(
			motor, VtorPulseMotor_PulsePin, motor->motorPulseLevel);
	}
	else
	{
		if(motor->motorState != VtorStepMotorState_Stop)
		{
			motor->motorState = VtorStepMotorState_Stop;
		}
	}
}

#endif // __VTOR_STEP_MOTOR__

